/*
Main source for for the AC motor controller.

Entry Point and initialization
*/

//system settings

//necessary for the mplabx auto source parser to properly parse the next include
#include <p30F4011.h>
#include <libpic30.h>
#include "settings.h"
#include "AC-Controller.h"
#include "globals.h"
#include "sin_table.h"

//prototypes for functions we might call
#include "hw_init.h"
#include "adc.h"
#include "acim_foc.h"
#include "acim_vhz.h"
#include "throttle.h"
#include "uart.h"
#include "canbus.h"
#include "eeprom.h"
#include "precharge.h"

/*****************Config bit settings****************/

// Use XT with external crystal from 4MHz to 10MHz.  FRM Pg. 178
//Turn off clock switching and monitoring. We might want to turn it on
_FOSC(XT_PLL8 & CSW_FSCM_OFF); 

// See Pg. 709 in F.R.M
//WDT is set off so that we can control it via software. This means
//that I really don't intend for it to always be off. OFF here means
//software controlled.
//Time period is 2ms * PrescaleA * PrescaleB
//2 * 64 * 4 = 512ms.
_FWDT(WDT_OFF & WDTPSA_64 & WDTPSB_4); 

// Brown Out voltage set to 4.2v.
// Power up time of 64 ms. 
// MCLR is enabled. 
// All PWM outputs are set active high because there is a NOT buffer
// on all outputs. Being a NOT buffer it makes everything actually active low
// like it really is at the driver side. But we've got to be backwards at the processor
_FBORPOR(BORV42 & PWRT_64 & MCLR_EN & PWMxL_ACT_HI & PWMxH_ACT_HI); 

_FGS(GWRP_OFF & CODE_PROT_OFF); //all code protection off

CtrlBitField ControllerState;

int use_hardware; //storage for which hardware to use

int main() {

	//during this initialization code the WDT is off. We'll turn it on later

	InitEEPROM();
	init_vhz();
	Motor_FOC_Init();
	InitIORegisters();
	InitTimer1();
	InitPWM();
	InitADC();
	InitQEI();
	InitUART2();
	InitCanbus();

	use_hardware = ee_get_use_devices();
	throttle_max_setting = ee_get_throttle_max();
	throttle_min_setting = ee_get_throttle_min();
	volt_gain = ee_get_voltage_gain();
	current_gain = ee_get_current_gain();
	current_bias = ee_get_current_bias();

	if (use_hardware & EE_USE_PRECHARGE) {
            if (precharge() == PRECHARGE_OK) {
               	if (use_hardware & EE_USE_MAIN_CONTACTOR)	{
                    SET_HIGH(CONTACTOR_RELAY);
                    delay_ms(250);
        	}
                ControllerState.Running = 1; //allow the controller to run
            }
            else {
                ControllerState.Running = 0; //stop motor control from working
            }
	}
	SET_LOW(PRECHARGE_RELAY);

	//We're done initializing and will now be running for real. Enable the watchdog timer
	//so that it resets the processor if something flubs up.
	RCONbits.SWDTEN = 1;


	/* 
	   Ideally, before the state is set to running, we should require that the throttle
	   be in the proper band to indicate that a throttle exists and is currently not pressed
	   also, the drive enable switch should start out off and then be turned on. It doesn't
	   count if it starts out on. This all makes sure that we're not about to go wildly
	   driving the car through a wall, over a dozen nuns, through an orphanage, and into
	   a lake.
	   
	   Of course, all of this should be configurable
    */
	ControllerState.FOC = 0; //locked to V/Hz for now

	while (1) { //it never ends! Buwahahaha!
		if (ControllerState.Running) {
			if (ControllerState.MotorCtrl) {
				if (ControllerState.FOC) MotorLoop_FOC();
					else MotorLoop_VHz();
				ControllerState.MotorCtrl = 0;
			}
		}
		if (ControllerState.PeriodicTasks) {
			ProcessThrottle();
			ControllerState.PeriodicTasks = 0;
		}
		if (ControllerState.SendDebug) {
			ControllerState.SendDebug = 0;
			canbus_send_status(); //send out our status frames
		}
		canbus_receive(); //get any waiting frames
		asm("CLRWDT"); //reset the watchdog timer. If you don't do this often enough you get your patties wacked
	}
}
